# -*- coding: utf-8 -*-
"""
Created on Tue Aug 17 19:46:12 2021

@author: 23119
"""

import face_recognition
import cv2
import numpy as np

import RPi.GPIO as GPIO
import time



def setServo(servo,angle):#每次传舵机引脚和角度
        print('servo is work')
        print('servo:%s,oldangle%s'%(servo,angle))
        GPIO.setmode(GPIO.BOARD)
        GPIO.setwarnings(False)
        GPIO.setup(servo,GPIO.OUT)
        p=GPIO.PWM(servo,50)
        p.start(0)
        p.ChangeDutyCycle(2.5+angle/360*20)
        time.sleep(0.2)
        p.stop()
        GPIO.cleanup()



setServo(32,50)
setServo(12,90)
angleBOTTOM=90
angleTOP=50
#初始化舵机都为90度

video_capture = cv2.VideoCapture(0)
#加载袁照片 并训练人脸
yuan_image = face_recognition.load_image_file("y1.jpg")

yuan_face_encoding = face_recognition.face_encodings(yuan_image)[0]

# 训练许照片
promise_image = face_recognition.load_image_file("x1.jpg")
promise_face_encoding = face_recognition.face_encodings(promise_image)[0]


#两个数组 一个用来记录已经学习的人脸  一个用来给学习过的人脸起名字 
known_face_encodings = [
    yuan_face_encoding,
    promise_face_encoding
]
known_face_names = [
    "yuan",
    "promise"
]

# Initialize some variables
face_locations = []
face_encodings = []
face_names = []
process_this_frame = True

while True:
    ret, frame = video_capture.read()
    ret=video_capture.set(3,640)
    ret=video_capture.set(4,480)

    # 对图像进行缩放 缩放为原来1/16 x轴y轴都变为原来1/4
    small_frame = cv2.resize(frame, (0, 0), fx=0.25, fy=0.25)

   
    #转换为rgb模式
    rgb_small_frame = small_frame[:, :, ::-1]

    # Only process every other frame of video to save time
    if process_this_frame:
        # Find all the faces and face encodings in the current frame of video
        face_locations = face_recognition.face_locations(rgb_small_frame)
        face_encodings = face_recognition.face_encodings(rgb_small_frame, face_locations)

        face_names = []
        print('face_locatinos%s'%(face_locations))
        for face_encoding in face_encodings:
            # See if the face is a match for the known face(s)
            matches = face_recognition.compare_faces(known_face_encodings, face_encoding)
            name = "Unknown"

            face_distances = face_recognition.face_distance(known_face_encodings, face_encoding)
            best_match_index = np.argmin(face_distances)
            if matches[best_match_index]:
                name = known_face_names[best_match_index]

            face_names.append(name)

    process_this_frame = not process_this_frame


    # Display the results
    for (top, right, bottom, left), name in zip(face_locations, face_names):
        top *= 4
        right *= 4
        bottom *= 4
        left *= 4

        # Draw a box around the face
        cv2.rectangle(frame, (left, top), (right, bottom), (0, 0, 255), 2)

        # Draw a label with a name below the face
        cv2.rectangle(frame, (left, bottom - 35), (right, bottom), (0, 0, 255), cv2.FILLED)
        font = cv2.FONT_HERSHEY_DUPLEX
        cv2.putText(frame, name, (left + 6, bottom - 6), font, 1.0, (255, 255, 255), 1)
        print('right/4%s-left/4:%s'%(right/4,left/4))
        print('bottom/4%s-top/4: %s'%(bottom/4,top/4))

        if 72<=left/4<=160 and right/4<=88<=160:
           pass
        else:
            if left/4 <=71:
                angleBOTTOM+=1
            else:
                angleBOTTOM-=1
        setServo(12,angleBOTTOM)
        
        if 54<=top/4<=120 and bottom/4<=66<=120:
                pass
        else:
                if top/4<=53:
                    angleTOP-=1
                else:
                    angleTOP+=1
        setServo(32,angleTOP)
                    
               
   
    cv2.imshow('Video', frame)

    
    if cv2.waitKey(1) & 0xFF == ord('q'):
        break
else:
    video_capture.release()
    cv2.destrotAllWindows()

video_capture.release()
cv2.destroyAllWindows()
